#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, PIDControl)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

task main()
{
  while(true)                            // Infinite loop:
  {
    getJoystickSettings(joystick);

    if(abs(joystick.joy1_y2) > 10)   // If the right analog stick's Y-axis readings are either above or below the Q:
    {
      motor[motorD] = -(joystick.joy1_y1);         // Motor D is assigned a power level equal to the right analog stick's Y-axis reading.
    }
    else
    {
      motor[motorD] = 0;                        // Motor D is stopped with a power level of 0.
    }


    if(abs(joystick.joy1_y1) > 10)   // If the left analog stick's Y-axis readings are either above or below the Q:
    {
      motor[motorE] = -(joystick.joy1_y2);         // Motor E is assigned a power level equal to the left analog stick's Y-axis reading.
    }
    else                                    // Else if the readings are within the Q:
    {
      motor[motorE] = 0;                        // Motor E is stopped with a power level of 0.
    }

  if(joy1Btn(6)) //Turns Ball Collector 'Tread' motor on
  {
    motor[motorF] = -100;
    servo[servo1] = 174;
    servo[servo2] = 68;
  }
 if(joy1Btn(5)) //Turns Ball Collector 'Tread' motor off
  {
    motor[motorF] = 0;
  }

  if(joy1Btn(1)) //Assigns X Button to move servos down
  {
    servo[servo1] = 250;
    servo[servo2] = 0;
  }
}
}
